#include "ezfb_pixels.h"
#include <math.h>
// convert pixel format from 
void ezfb_conv_pixel(struct ezfb * fb, u_int * cres, u_int csrc, int src_mode) 
{

  *cres = 0 ; 
  switch(fb->scr_bpp) {
  case 1:
  case 2:
  case 4:
  case 8:
  case 15:
  case 16:
	  {
	  }
	  break ; 
  case 18:
    {
      // here used packed 18bpp : 000000rr rrrrgggg ggbbbbbb
      // it seems that motorola use packed 18bpp not byte aligned 18 bpp, 
	  // >:-() that may slow down the execute speed 
	  // packed 18bpp       : 000000rr rrrrgggg ggbbbbbb
	  // byte aligned 18bpp : 00rrrrrr 00gggggg 00bbbbbb
	  // the asign value could be the same with 24bpp
	  // but remember the u_int c must be packed 18bpp format 
      switch(src_mode) {
      case MODE_BPP8:
      case MODE_BPP16:
		{
	  		// rgb565 mode  
	  		*cres = ((csrc & 0x0001f) << 1) | 
			  ((csrc & 0x007e0) << 1) | 
			  ((csrc & 0x0f800) << 2) ; 
	  		break ; 
		}
      case MODE_BPP24:
		{
	  		// rgb888 mode 
	  		*cres = ((csrc & 0x00000ff) >> 2) | 
			  ((csrc & 0x000ff00) >> 4) | 
			  ((csrc & 0x0ff0000) >> 4) ; 
	  		break ; 
		}
      case MODE_BPP32:
      default:
		break ; 
      }
      break ; 
    }
  case 24:
  case 32:
  default:
    break ; 
  }
}

//############################################################################
int ezfb_put_pixel(struct ezfb* fb, u_int  x, u_int  y, u_int  c)
{
    if(fb->dds_ptr && (x < fb->scr_width) && (y < fb->scr_height)) {
		register int idx ; 
		if(EZFB_LOCK(fb) != DD_OK) return EZFB_FALSE ;
		EZFB_MAP(fb) ;
        switch(fb->scr_bpp) {
			case  1: break ;
			case  2: break ;
			case  4: break ; 
			case  8: break ; 
				{
					u_char * pscr = (u_char *)(fb->vga_ptr) ; 
					idx = y * fb->scr_width + x ;  
					pscr[idx] = (u_char)(c) ; 
					break;
				}	 
			case 15:
			case 16: 
				{
					idx = y * fb->scr_width + x ; 
					fb->vga_ptr[idx] = (u_short)(c) ;
					break;
				}
			case 18: 
			case 24:
				{
					// rgb format is rrrrrrrr gggggggg bbbbbb
					u_char * pscr = (u_char *)(fb->vga_ptr) ; 
					idx = y * fb->scr_width + x * 3 ; 
					pscr[idx++] = (u_char)(c & 0x00000ff) ; 
					pscr[idx++] = (u_char)((c & 0x000ff00) >> 0x08) ; 
					pscr[idx++] = (u_char)((c & 0x0ff0000) >> 0x10) ; 
					break ; 
				}
			case 32: 
				{
					u_int * pscr = (u_int *)(fb->vga_ptr) ; 
					idx = y * fb->scr_width + x ; 
					pscr[idx] = c ; 
					break;
				}
			default: ;
        }
		EZFB_UNLOCK(fb) ; 
        return EZFB_TRUE ; 
    }
	return EZFB_FALSE ; 
}

//############################################################################
u_int ezfb_get_pixel(struct ezfb* fb, u_int  x, u_int  y)
{
	union {
		u_char a[4] ;
		u_int  val ; 
	} data ; 
	
	data.val = 0 ; 

	if(fb->dds_ptr && (x < fb->scr_width) && (y < fb->scr_height)) {
		register int idx ; 
		if(EZFB_LOCK(fb) != DD_OK) return EZFB_FALSE ;
		EZFB_MAP(fb) ;
		switch(fb->scr_bpp) {
			case  1: break ;
			case  2: break ;
			case  4: break ; 
			case  8: break ; 
				{
					u_char * pscr = (u_char *)(fb->vga_ptr) ; 
					idx = y * fb->scr_width + x ;  
					data.val = pscr[idx] ; 
					break;
				}	 
			case 15:
			case 16: 
				{
					idx = y * fb->scr_width + x ; 
					data.val = fb->vga_ptr[idx] ; 
					break;
				}
			case 18: 
			case 24:
				{
					// rgb format is rrrrrrrr gggggggg bbbbbb
					u_char * pscr = (u_char *)(fb->vga_ptr) ; 
					idx = y * fb->scr_width + x * 3 ; 
					data.a[0] = pscr[idx++] ; 
					data.a[1] = pscr[idx++] ; 
					data.a[2] = pscr[idx++] ; 
					break ; 
				}
			case 32: 
				{
					u_int * pscr = (u_int *)(fb->vga_ptr) ; 
					idx = y * fb->scr_width + x ; 
					data.val = pscr[idx] ; 
					break;
				}
			default: 
				break ; 
		}
		EZFB_UNLOCK(fb) ; 
		return data.val ; 
	}
	return data.val ; 
}

//############################################################################
int ezfb_put_line(struct ezfb* fb, int x1, int y1, int x2, int y2, u_int c)
{
    int dx  = x2 - x1;
    int dy  = y2 - y1;
    int n, b;
    float m;

    if(0 == dx) {
        if(dy < 0) {
            for(n = y1; n >= y2; n--)
				ezfb_put_pixel(fb, x1, n, c);
        } else {
            for(n = y1; n <= y2; n++)
                ezfb_put_pixel(fb, x1, n, c);
        }
    } else if(0 == dy) {
        if(dx < 0) {
            for(n = x1; n >= x2; n--)
                ezfb_put_pixel(fb, n, y1, c);
        } else {
            for(n = x1; n <= x2; n++)
                ezfb_put_pixel(fb, n, y1, c);
        }
    } else if(abs(dx) >= abs(dy)) {
        m = ((float)dy) / ((float)dx);
        b = (int)(y1 - m * x1);
        if(dx < 0) {
            for(n = x1; n >= x2; n--)
                ezfb_put_pixel(fb, n, (u_int)floor(n * m + b + 0.5), c);
        } else { 
            for(n = x1; n <= x2; n++)
                ezfb_put_pixel(fb, n, (u_int)floor(n * m + b + 0.5), c);
        }
    } else {
        m = ((float)dx) / ((float)dy);
        b = (int)(x1 - m * y1);
        if(dy < 0) {
            for(n = y1; n >= y2; n--)
                ezfb_put_pixel(fb, (u_int)floor(n * m + b + 0.5), n, c);
        } else {
            for(n = y1; n <= y2; n++)
                ezfb_put_pixel(fb, (u_int)floor(n * m + b + 0.5), n, c);
        }
    }
	return EZFB_TRUE ; 
}

//############################################################################
int ezfb_put_rectangle(struct ezfb* fb, int x1, int y1, int x2, int y2, u_int c, int filled)
{
    if(filled) {
        int y, step;
        step = (y1 <= y2) ? 1 : -1;
        if((x1 != x2) && (y1 != y2))
            for(y = y1; y <= y2; y += step)
                ezfb_put_line(fb, x1, y, x2, y, c);
        else if(x1 != x2)
            ezfb_put_line(fb, x1, y1, x2, y1, c);
        else
            ezfb_put_pixel(fb, x1, y1, c);
    } else {
        if((x1 != x2) && (y1 != y2)) {
            ezfb_put_line(fb, x1, y1, x2, y1, c);
            ezfb_put_line(fb, x1, y2, x2, y2, c);
            ezfb_put_line(fb, x1, y1, x1, y2, c);
            ezfb_put_line(fb, x2, y1, x2, y2, c);
        }
        else if(x1 != x2) ezfb_put_line(fb, x1, y1, x2, y1, c);
        else ezfb_put_pixel(fb, x1, y1, c);
    }
	return EZFB_TRUE ; 
}

int ezfb_put_ellipse(struct ezfb* fb, int x1, int y1, int x2, int y2, u_int c, int filled)
{
	int elip_width = (x2 - x1) / 2 ;
	int elip_height = (y2 - y1) / 2 ;  
	int center_x = elip_width + x1 ; 
	int center_y = elip_height + y1 ; 
	u_int radius_x = center_x - x1 ; // maybe changed later 
	u_int radius_y = center_y - y1 ;
	ezfb_point pt; 
	int p ; 

	int minYofDeltaX = (int)floor(double(radius_y*radius_y/
						          sqrt(radius_y*radius_y+radius_x*radius_x))) + 1;

	pt.x = 0; 
	pt.y = radius_y; 

	p = radius_y*radius_y - 
		radius_x*radius_x*radius_y + 
		1/4*(radius_x*radius_x); 
		
	while(minYofDeltaX < pt.y) { 
		if( p < 0) { 
			++pt.x ; 
			p = p + radius_y*radius_y*(3+2*pt.x); 
		} else { 
			++pt.x; 
			--pt.y; 
			p = p + radius_y*radius_y*(3+2*pt.x) - 2*radius_x*radius_x*(pt.y-1) ; 
		}
		if(!filled) {
			ezfb_put_pixel(fb, center_x+pt.x, center_y+pt.y, c);
			ezfb_put_pixel(fb, center_x+pt.x, center_y-pt.y, c);
			ezfb_put_pixel(fb, center_x-pt.x, center_y+pt.y, c);
			ezfb_put_pixel(fb, center_x-pt.x, center_y-pt.y, c);
		} else {
			ezfb_put_line(fb, 
				      center_x+pt.x, center_y+pt.y, 
				      center_x+pt.x, center_y-pt.y, c) ; 
			ezfb_put_line(fb, 
				      center_x-pt.x, center_y+pt.y, 
				      center_x-pt.x, center_y-pt.y, c) ; 
		}
	}
	
	if(!filled) {
		ezfb_put_pixel(fb, center_x, center_y-elip_height, c);
		ezfb_put_pixel(fb, center_x, center_y+elip_height, c);
	} else ezfb_put_line(fb, 
						 center_x, center_y-elip_height, 
						 center_x, center_y+elip_height, c) ; 
	
	p = radius_y*radius_y*(0+1/2)*(0+1/2) + 
		radius_x*radius_x*(radius_y-1)*(radius_y-1) - 
		radius_x*radius_x*radius_y*radius_y; 

	while(pt.y > 0) { 
		if(p > 0) { 
			--pt.y; 
			p = p - 2*radius_x*radius_x*pt.y - 
				2*radius_x*radius_x + radius_x*radius_x; 
		} else { 
			--pt.y; 
			++pt.x; 
			p = p - 2*radius_x*radius_x*pt.y - 
				2*radius_x*radius_x + radius_x*radius_x + 
				2*radius_y*radius_y*pt.x + 2*radius_y*radius_y ; 
		} 
		if(!filled) {
			ezfb_put_pixel(fb, center_x+pt.x, center_y+pt.y, c);
			ezfb_put_pixel(fb, center_x+pt.x, center_y-pt.y, c);
			ezfb_put_pixel(fb, center_x-pt.x, center_y+pt.y, c);
			ezfb_put_pixel(fb, center_x-pt.x, center_y-pt.y, c);
		} else {
			ezfb_put_line(fb, 
				      center_x+pt.x, center_y+pt.y, 
				      center_x+pt.x, center_y-pt.y, c) ; 
			ezfb_put_line(fb, 
				      center_x-pt.x, center_y+pt.y, 
				      center_x-pt.x, center_y-pt.y, c) ; 
		}
	} 
	return EZFB_TRUE ; 
}

void ezfb_put_screen(struct ezfb* fb, u_char * mem_buf, int buf_mode) 
{
	if(EZFB_LOCK(fb) != DD_OK) return ; 
	EZFB_MAP(fb) ;
	switch(fb->scr_bpp) {
	case 8:
	case 16:
		{
			switch(buf_mode) {
				case MODE_BPP8:
				case MODE_BPP16:
					{
						u_short * 	src = (u_short *)(mem_buf) ;  
						memcpy(fb->vga_ptr, src, fb->scr_width * fb->scr_height * 2) ; 
						break ; 
					}
				case MODE_BPP24:
				case MODE_BPP32:
				default:
					break ; 
			}
		}
		break ; 
	case 18:
		{
			int w = fb->scr_width ; 
			int h = fb->scr_height ; 
			int i, j ; 
			register union {
				    u_char a[4] ;
				    u_int  clr ;  
    		} v ;

			switch(buf_mode) {
			case MODE_BPP8:
			case MODE_BPP16:
				{
				  // convert 16bpp mem buffer to packed 18bpp
				  u_short * src = (u_short *)(mem_buf) ;  
				  u_char *  dst = (u_char *)(fb->vga_ptr) ; 
				  for(i = 0 ; i < h ; i++) {
				    for(j = 0 ; j < w ; j++) {
				      v.clr = *src++ ; 
				      // convert rgb565 to rgb666 packed
				      v.clr = ((v.clr & 0x0001f) << 1) | 
					((v.clr & 0x007e0) << 1) | 
					((v.clr & 0x0f800) << 2) ; 

				      *dst++ = v.a[0] ; 
				      *dst++ = v.a[1] ; 
				      *dst++ = v.a[2] ; 
				    }
				  }
				  break ; 
				}
			case MODE_BPP24:
			case MODE_BPP32:
			default:
			break ; 
			}
			break ; 
		}
	case 24:
	case 32:
	default:
		break ; 
	}
	EZFB_UNLOCK(fb) ; 
}

void ezfb_blt_screen(struct ezfb* fb, u_char * mem_buf, 
					 int buf_mode, int x, int y, int w, int h) 
{
	int src_pitch = 0, dst_pitch = 0 ;
	int src_jump = 0, dst_jump = 0 ;
	int i, j ; 

	if(EZFB_LOCK(fb) != DD_OK) return ; 
	EZFB_MAP(fb) ;
	switch(fb->scr_bpp) {
	case 8:
	case 16:
		{
			switch(buf_mode) {
			case MODE_BPP8: break ; 
			case MODE_BPP16:
				{
					src_pitch = w % 2 ; // 16bpp need 2 bytes align 
					dst_jump = (fb->scr_width - w) ; 
					u_short * 	src = (u_short *)(mem_buf) ;  
					u_short * 	dst = fb->vga_ptr ; 
					dst += (y * fb->scr_width + x) ; 
					for(i = 0 ; i < h ; i++) {
						memcpy((u_short*)(dst), src, w * 2) ;
						dst += w ; 
						src += w ;  
						src += src_pitch ; 
						dst += dst_jump ; 
					}
				}
				break ; 
			case MODE_BPP24: 
				{
					src_pitch = (w * 3) % 4;
					dst_pitch = (fb->scr_width) % 4;
					if (src_pitch)
						src_pitch = 4 - src_pitch;
					if (dst_pitch)
						dst_pitch = 4 - dst_pitch;
					src_jump = w * 3 + dst_pitch;
					dst_jump = fb->scr_width + dst_pitch;
					u_short * src = (u_short *)(mem_buf);
					u_short * dst = fb->vga_ptr ; 
					dst += (y * dst_jump + x);
					for(i = 0 ; i < h ; i++) {
						for (j = 0; j < w; j++)
						{
							WORD rbg565 = RGB888to565(src[j*3+2], src[j*3+1], src[j*3]);

							memcpy(dst + j, (void *)&rbg565, 2);
						}
						src += src_jump;
						dst += dst_jump;
					}
				}
				break;
			case MODE_BPP32:
			default:
				break ; 
			}
		}
		break ; 
	case 18:
		{
			switch(buf_mode) {
			case MODE_BPP8:
			case MODE_BPP16:
				{
				  register union {
					u_char a[4] ;
					u_int  clr ;  
    			  } v ; 
				  
				  // convert 16bpp mem buffer to packed 18bpp
				  src_pitch = w % 2 ; // 16bpp need 2 bytes align 
				  dst_jump = (fb->scr_width- w) * 3 ; 
				  u_short * src = (u_short *)(mem_buf) ;  
				  u_char * dst = (u_char *)(fb->vga_ptr) ; 
				  dst += (y * fb->scr_width * 3 + x * 3) ; 
				  for(i = 0 ; i < h ; i++) {
				    for(j = 0 ; j < w ; j++) {
				      v.clr = *src++ ;
				      // convert rgb565 to rgb666 packed
				      v.clr = ((v.clr & 0x0001f) << 1) | 
							  ((v.clr & 0x007e0) << 1) | 
							  ((v.clr & 0x0f800) << 2) ; 

				      *dst++ = v.a[0] ; 
				      *dst++ = v.a[1] ; 
				      *dst++ = v.a[2] ; 
				    }
				    src += src_pitch ; 
				    dst += dst_jump ; 
				  }
				  break ; 
				}
			case MODE_BPP24:
			case MODE_BPP32:
			default:
			break ; 
			}
			break ; 
		}
	case 24:
	case 32:
	default:
		break ; 
	}
	EZFB_UNLOCK(fb) ; 
}

void ezfb_alpha_screen(struct ezfb* fb, u_char * mem_buf, u_char * mem_alpha, 
					 int buf_mode, int x, int y, int w, int h) 
{
	int src_pitch = 0, dst_pitch = 0 ;
	int src_jump = 0, dst_jump = 0 ;
	int i, j ; 

	if(EZFB_LOCK(fb) != DD_OK) return ; 
	EZFB_MAP(fb) ;
	switch(fb->scr_bpp) {
	case 8:
	case 16:
		{
			switch(buf_mode) {
	case MODE_BPP8: break ; 
	case MODE_BPP16:
		{
			src_pitch = w % 2 ; // 16bpp need 2 bytes align 
			dst_jump = (fb->scr_width - w) ; 
			u_short * 	src = (u_short *)(mem_buf) ;  
			u_short * 	dst = fb->vga_ptr ; 
			dst += (y * fb->scr_width + x) ; 
			for(i = 0 ; i < h ; i++) {
				memcpy((u_short*)(dst), src, w * 2) ;
				dst += w ; 
				src += w ;  
				src += src_pitch ; 
				dst += dst_jump ; 
			}
		}
		break ; 
	case MODE_BPP24: 
		{
			src_pitch = (w * 3) % 4;
			dst_pitch = (fb->scr_width) % 4;
			if (src_pitch)
				src_pitch = 4 - src_pitch;
			if (dst_pitch)
				dst_pitch = 4 - dst_pitch;
			src_jump = w * 3 + dst_pitch;
			dst_jump = fb->scr_width + dst_pitch;
			u_short * src = (u_short *)(mem_buf);
			u_short * dst = fb->vga_ptr ; 
			dst += (y * dst_jump + x);
			for(i = 0 ; i < h ; i++) {
				for (j = 0; j < w; j++)
				{
					unsigned char alpha = mem_alpha[i * w + j];
					unsigned char r1, g1, b1; // src
					unsigned char r2, g2, b2; // dst
					WORD c1; // src
					WORD c2; // dst

					r1 = src[j*3+2];
					g1 = src[j*3+1];
					b1 = src[j*3];

					memcpy((void *)&c2, dst + j, 2);

					r2 = RGB565to888ValueR(c2);
					g2 = RGB565to888ValueG(c2);
					b2 = RGB565to888ValueB(c2);

					// alpha
					r2 += ((alpha * (r1 - r2)) >> 8);
					g2 += ((alpha * (g1 - g2)) >> 8);
					b2 += ((alpha * (b1 - b2)) >> 8);

					c2 = RGB888to565(r2, g2, b2);

					memcpy(dst + j, (void *)&c2, 2);
				}
				src += src_jump;
				dst += dst_jump;
			}
		}
		break;
	case MODE_BPP32:
		{
			u_short * 	src = (u_short *)(mem_buf) ;  
			u_short * 	dst = fb->vga_ptr ; 
			dst += (y * fb->scr_width + x) ; 
			for(i = 0 ; i < h ; i++) {
				memcpy((u_short*)(dst), src, w * 3) ;
				dst += w ; 
				src += w ;  
			}
		}
		break ;
	default:
		break ; 
			}
		}
		break ; 
	case 18:
		{
			switch(buf_mode) {
	case MODE_BPP8:
	case MODE_BPP16:
		{
			register union {
				u_char a[4] ;
				u_int  clr ;  
			} v ; 

			// convert 16bpp mem buffer to packed 18bpp
			src_pitch = w % 2 ; // 16bpp need 2 bytes align 
			dst_jump = (fb->scr_width- w) * 3 ; 
			u_short * src = (u_short *)(mem_buf) ;  
			u_char * dst = (u_char *)(fb->vga_ptr) ; 
			dst += (y * fb->scr_width * 3 + x * 3) ; 
			for(i = 0 ; i < h ; i++) {
				for(j = 0 ; j < w ; j++) {
					v.clr = *src++ ;
					// convert rgb565 to rgb666 packed
					v.clr = ((v.clr & 0x0001f) << 1) | 
						((v.clr & 0x007e0) << 1) | 
						((v.clr & 0x0f800) << 2) ; 

					*dst++ = v.a[0] ; 
					*dst++ = v.a[1] ; 
					*dst++ = v.a[2] ; 
				}
				src += src_pitch ; 
				dst += dst_jump ; 
			}
			break ; 
		}
	case MODE_BPP24:
	case MODE_BPP32:
	default:
		break ; 
			}
			break ; 
		}
	case 24:
	case 32:
	default:
		break ; 
	}
	EZFB_UNLOCK(fb) ; 
}
